
// import oimo.collision.narrowphase.*;
// import oimo.common.*;
// import oimo.dynamics.*;
// import oimo.dynamics.TimeStep;
// import oimo.dynamics.constraint.*;
// import oimo.dynamics.constraint.info.*;
// import oimo.dynamics.constraint.info.contact.*;
// import oimo.dynamics.constraint.solver.*;
// import oimo.dynamics.constraint.solver.pgs.*;
// import oimo.dynamics.rigidbody.*;
// import oimo.m.Mat3;
// import oimo.m.Vec3;
// import oimo.m.M;

import { Manifold } from "./Manifold";
import { Shape } from "../../rigidbody/Shape";
import { Transform } from "../../../common/Transform";
import { Mat3 } from "../../../common/Mat3";
import { RigidBody } from "../../rigidbody/RigidBody";
import { Vec3 } from "../../../common/Vec3";
import { M } from "../../../common/M";
import { MathUtil } from "../../../common/MathUtil";
import { ManifoldPoint } from "./ManifoldPoint";
import { Setting } from "../../../common/Setting";
import { PositionCorrectionAlgorithm } from "../PositionCorrectionAlgorithm";
import { TimeStep } from "../../TimeStep";
import { ConstraintSolver } from "../ConstraintSolver";
import { PgsContactConstraintSolver } from "../solver/pgs/PgsContactConstraintSolver";
import { ContactSolverInfo } from "../info/contact/ContactSolverInfo";
import { ContactSolverInfoRow } from "../info/contact/ContactSolverInfoRow";
import { JacobianRow } from "../info/JacobianRow";

/**
 * A contact constraint provides collision information for a contact constraint solver.
 * This holds a contact manifold, which has some contact points, contact normals, and
 * contact impulses. See `Manifold` for more information.
 */
export class ContactConstraint {
	_positionCorrectionAlgorithm: number;

	_manifold: Manifold;

	_s1: Shape;
	_s2: Shape;
	_tf1: Transform;
	_tf2: Transform;
	_invM1: number;
	_invM2: number;
	_friction: number;
	_restitution: number;

	_invI1: Mat3;
	_invI2: Mat3;

	_b1: RigidBody;
	_b2: RigidBody;

	_solver: ConstraintSolver;


	constructor(manifold: Manifold) {
		this._solver = new PgsContactConstraintSolver(this);
		this._manifold = manifold;
	}

	// --- internal ---


	public _attach(s1: Shape, s2: Shape): void {
		this._s1 = s1;
		this._s2 = s2;
		this._b1 = this._s1._rigidBody;
		this._b2 = this._s2._rigidBody;
		this._tf1 = this._b1._transform;
		this._tf2 = this._b2._transform;
	}


	public _detach(): void {
		this._s1 = null;
		this._s2 = null;
		this._b1 = null;
		this._b2 = null;
		this._tf1 = null;
		this._tf2 = null;
	}

	public _getVelocitySolverInfo(timeStep: TimeStep, info: ContactSolverInfo): void {
		info.b1 = this._b1;
		info.b2 = this._b2;

		var normal: Vec3 = this._manifold._normal.clone();
		var tangent: Vec3 = this._manifold._tangent.clone();
		var binormal: Vec3 = this._manifold._binormal.clone();

		var friction: number = MathUtil.sqrt(this._s1._friction * this._s2._friction);
		var restitution: number = MathUtil.sqrt(this._s1._restitution * this._s2._restitution);

		var num: number = this._manifold._numPoints;
		info.numRows = 0;

		var posDiff: Vec3 = new Vec3();
		M.vec3_sub(posDiff, this._tf1._position, this._tf2._position);

		for (let i = 0; i < num; ++i) {
			var p: ManifoldPoint = this._manifold._points[i];

			if (p._depth < 0) {
				p._disabled = true;

				// clear accumulated impulses
				p._impulse.clear();

				// skip separated points
				continue;
			} else {
				p._disabled = false;
			}

			var row: ContactSolverInfoRow = info.rows[info.numRows++];

			row.friction = friction;
			row.cfm = 0; // TODO: implement APIs for CFM setting?

			// set Jacobian
			var j: JacobianRow;

			j = row.jacobianN;
			M.vec3_assign(j.lin1, normal);
			M.vec3_assign(j.lin2, normal);
			M.vec3_cross(j.ang1, p._relPos1, normal);
			M.vec3_cross(j.ang2, p._relPos2, normal);

			j = row.jacobianT;
			M.vec3_assign(j.lin1, tangent);
			M.vec3_assign(j.lin2, tangent);
			M.vec3_cross(j.ang1, p._relPos1, tangent);
			M.vec3_cross(j.ang2, p._relPos2, tangent);

			j = row.jacobianB;
			M.vec3_assign(j.lin1, binormal);
			M.vec3_assign(j.lin2, binormal);
			M.vec3_cross(j.ang1, p._relPos1, binormal);
			M.vec3_cross(j.ang2, p._relPos2, binormal);

			// compute relative velocity
			j = row.jacobianN;
			var rvn: number =
				(M.vec3_dot(j.lin1, this._b1._vel) + M.vec3_dot(j.ang1, this._b1._angVel)) -
				(M.vec3_dot(j.lin2, this._b2._vel) + M.vec3_dot(j.ang2, this._b2._angVel))
				;

			// disable bounce for warm-started contacts
			if (rvn < -Setting.contactEnableBounceThreshold && !p._warmStarted) {
				row.rhs = -rvn * restitution;
			} else {
				row.rhs = 0;
			}

			// set minimum RHS for baumgarte position correction
			if (this._positionCorrectionAlgorithm == PositionCorrectionAlgorithm.BAUMGARTE) {
				if (p._depth > Setting.linearSlop) {
					var minRhs: number = (p._depth - Setting.linearSlop) * Setting.velocityBaumgarte * timeStep.invDt;
					if (row.rhs < minRhs) row.rhs = minRhs;
				}
			}

			// reset impulses if warm starting is disabled
			if (!p._warmStarted) {
				p._impulse.clear();
			}

			row.impulse = p._impulse;
		}
	}

	public _getPositionSolverInfo(info: ContactSolverInfo): void {
		info.b1 = this._b1;
		info.b2 = this._b2;

		var normal: Vec3 = new Vec3();
		M.vec3_assign(normal, this._manifold._normal);

		var num: number = this._manifold._numPoints;
		info.numRows = 0;

		for (let i = 0; i < info.numRows; ++i) {
			var p: ManifoldPoint = this._manifold._points[i];

			if (p._disabled) {
				continue; // skip disabled points
			}

			var row: ContactSolverInfoRow = info.rows[info.numRows++];

			// set normal Jacobian
			var j: JacobianRow = row.jacobianN;
			M.vec3_assign(j.lin1, normal);
			M.vec3_assign(j.lin2, normal);
			M.vec3_cross(j.ang1, p._relPos1, normal);
			M.vec3_cross(j.ang2, p._relPos2, normal);

			row.rhs = p._depth - Setting.linearSlop;
			if (row.rhs < 0) {
				row.rhs = 0;
			}

			row.impulse = p._impulse;
		}
	}

	// !! don't forget to call this from constraint solvers !!
	public _syncManifold(): void {
		this._manifold._updateDepthsAndPositions(this._tf1, this._tf2);
	}

	// --- public ---

	/**
	 * Returns the first shape of the contact.
	 */
	public getShape1(): Shape {
		return this._s1;
	}

	/**
	 * Returns the second shape of the contact.
	 */
	public getShape2(): Shape {
		return this._s2;
	}

	/**
	 * Returns the contact manifold.
	 */
	public getManifold(): Manifold {
		return this._manifold;
	}

	/**
	 * Returns whether the two rigid bodies are touching.
	 */
	public isTouching(): boolean {
		for (let i = 0; i < this._manifold._numPoints; ++i) {
			if (this._manifold._points[i]._depth >= 0) return true;
		}
		return false;
	}

}
